#!/usr/bin/python3
import warnings

import threading
import numpy as np
from GlobVarLock import GlobVarLocker as glob

is_openLog = 0

class IMUDataExtracter(threading.Thread):
    def __init__(self):
        threading.Thread.__init__(self)

    def run(self):
        while(True):
            if(glob.getOriginDataRBLen()>40):
                glob.lockOriginDataRB()
                # glob.printOriginDataRB()
                # deal with origin data, output 1 slice one-type data
                if(glob.getOriginDataRBLen()>40):
                    self.extractData()
                glob.unlockOriginDataRB()
            else:
                pass

    def extractData(self):
        # check 1st & 2nd byte
        msgList = []  # for check sum
        dataList = []  # for data conversion
        if (0x55 == glob.popOriginDataRB()):
            msgList.append(0x55)
            if (0x55 == glob.popOriginDataRB()):
                msgList.append(0x55)
                # get ID
                id = glob.popOriginDataRB()
                dataLen = glob.popOriginDataRB()
                msgList.append(id)
                msgList.append(dataLen)
                for i in range(0, dataLen):
                    tempData = glob.popOriginDataRB()
                    msgList.append(tempData)
                    dataList.append(tempData)
                checkSum = glob.popOriginDataRB()
                msgList.append(checkSum)
                # check checkSum
                checkSumVal = 0
                for i in range(0, (len(msgList) - 1)):
                    checkSumVal = checkSumVal + msgList[i]
                checkSumVal = checkSumVal & 0xff  # only 8-bit data width
                # print(hex(checkSumVal), hex(checkSum))
                if (checkSum != checkSumVal):
                    warnings.warn("msg check sum error")
                else:
                    # print(msgList)
                    # check id, for analyze hex data
                    # audio=np.fromstring（原始数据，dtype=np.int16）
                    if (id == 0x01):  # pose data
                        # RollL RollH PitchL PitchH YawL YawH
                        # 计算方法：
                        # 滚转角（X 轴） Roll(float) = (float) ( (int16_t) (RollH<<8) | RollL ) / 32768 * 180 (°)
                        # 俯仰角（Y 轴） Pitch(float) = (float) ( (int16_t) (PitchH<<8) | PitchL ) / 32768 * 180 (°)
                        # 偏航角（Z 轴） Yaw(float) = (float) ( (int16_t) (YawH<<8) | YawL ) / 32768 * 180 (°)
                        roll = float(np.int16((dataList[1] << 8) | dataList[0])) / 32768 * 180
                        pitch = float(np.int16((dataList[3] << 8) | dataList[2])) / 32768 * 180
                        yaw = float(np.int16((dataList[5] << 8) | dataList[4])) / 32768 * 180
                        if (is_openLog == 1):
                            print("pose:", end=' ')
                            print(roll, pitch, yaw)

                        # put into ring buffer
                        glob.lockPoseDataRB()
                        glob.putPoseDataRB([roll, pitch, yaw])
                        glob.unlockPoseDataRB()

                    elif (id == 0x02):  # Quaternion
                        # Q0L Q0H Q1L Q1H Q2L Q2H Q3L Q3H
                        # 计算方法：
                        # q0(float) = (float) ( (int16_t) (Q0H<<8) | Q0L) / 32768 （范围 0.0~1.0）
                        # q1(float) = (float) ( (int16_t) (Q1H<<8) | Q1L) / 32768 （范围 0.0~1.0）
                        # q2(float) = (float) ( (int16_t) (Q2H<<8) | Q2L) / 32768 （范围 0.0~1.0）
                        # q3(float) = (float) ( (int16_t) (Q3H<<8) | Q3L) / 32768 （范围 0.0~1.0）
                        q0 = float(np.int16((dataList[1] << 8) | dataList[0])) / 32768
                        q1 = float(np.int16((dataList[3] << 8) | dataList[2])) / 32768
                        q2 = float(np.int16((dataList[5] << 8) | dataList[4])) / 32768
                        q3 = float(np.int16((dataList[7] << 8) | dataList[6])) / 32768
                        if (is_openLog == 1):
                            print("Quaternion:", end=' ')
                            print(q0, q1, q2, q3)

                        # put into ring buffer
                        glob.lockQuatDataRB()
                        glob.putQuatDataRB([q0, q1, q2, q3])
                        glob.unlockQuatDataRB()

                    elif (id == 0x03):  # gyro and acc data
                        # AxL AxH AyL AyH AzL AzH
                        # GxL GxH GyL GyH GzL GzH
                        # acc
                        # 加速度计原始数据：
                        # Ax(int16_t) = (int16_t) (AxH<<8) | AxL;
                        # 转换为加速度（单位G） :
                        # ACCx(float) = (float) Ax / 32768 * 4; 说明： 4 = 用户设置的量程 G， 1G 表示 9.8m/s2
                        accx = float(np.int16((dataList[1] << 8) | dataList[0])) / 32768 * 4
                        accy = float(np.int16((dataList[3] << 8) | dataList[2])) / 32768 * 4
                        accz = float(np.int16((dataList[5] << 8) | dataList[4])) / 32768 * 4
                        if (is_openLog == 1):
                            print("acc:", end=' ')
                            print(accx, accy, accz)

                        # put into ring buffer
                        glob.lockAccDataRB()
                        glob.putAccDataRB([accx, accy, accz])
                        glob.unlockAccDataRB()

                        # 陀螺仪原始数据：
                        # Gx(int16_t) = (int16_t) (GxH<<8) | GxL;
                        # 换为旋转速率（单位° /S）
                        # gyrox(float) = (float) Gx / 32768 * 2000;
                        # 说明： 2000 = 用户设置的量程， 单位为° /S。
                        gyrox = float(np.int16((dataList[7] << 8) | dataList[6])) / 32768 * 2000
                        gyroy = float(np.int16((dataList[9] << 8) | dataList[8])) / 32768 * 2000
                        gyroz = float(np.int16((dataList[11] << 8) | dataList[10])) / 32768 * 2000
                        if (is_openLog == 1):
                            print("gyro:", end=' ')
                            print(gyrox, gyroy, gyroz)

                        # put into ring buffer
                        glob.lockGyroDataRB()
                        glob.putGyroDataRB([gyrox, gyroy, gyroz])
                        glob.unlockGyroDataRB()


                    elif (id == 0x04):  # Magnetometer data
                        # MxL MxH MyL MyH MzL MzH TL TH
                        # 磁场计算方法：
                        # 磁场（X 轴） Mx(int16_t) = (int16_t) (MxH<<8) | MxL;
                        # 温度计算方法(℃)：
                        # 温度 T(float) = (float) ( (int16_t) (TH<<8) | TL ) / 100;
                        magnx = np.int16((dataList[1] << 8) | dataList[0])
                        magny = np.int16((dataList[3] << 8) | dataList[2])
                        magnz = np.int16((dataList[5] << 8) | dataList[4])
                        if (is_openLog == 1):
                            print("magn:", end=' ')
                            print(magnx, magny, magnz)

                        # put into ring buffer
                        glob.lockMagnDataRB()
                        glob.putMagnDataRB([magnx, magny, magnz])
                        glob.unlockMagnDataRB()

                        temperature0 = float(np.int16((dataList[7] << 8) | dataList[6])) / 100
                        if (is_openLog == 1):
                            print("T0:", end=' ')
                            print(temperature0)

                    elif (id == 0x05):  # Barometer data
                        # P0 P1 P2 P3 A0 A1 A2 A3 TL TH
                        # 气压值（单位 Pa）
                        # P(int32_t) = (int32_t) ( P3<<24) | ( P2<<16) | ( P1<<8) | P0;
                        pressure = np.int32((dataList[3] << 24) | (dataList[2] << 16) | (dataList[1] << 8) | dataList[0])
                        if (is_openLog == 1):
                            print("pressure:", end=' ')
                            print(pressure)
                        # 海拔高度（单位 cm）
                        # A (int32_t) = (int32_t) ( A3<<24) | ( A2<<16) | ( A1<<8) | A0;
                        altitude = np.int32((dataList[7] << 24) | (dataList[6] << 16) | (dataList[5] << 8) | dataList[4])
                        if (is_openLog == 1):
                            print("altitude:", end=' ')
                            print(altitude)
                        # 温度(℃)
                        # T(float) = (float) ( (int16_t) (AH<<8) | AL) / 100;
                        temperature1 = float(np.int16((dataList[9] << 8) | dataList[8])) / 100
                        if (is_openLog == 1):
                            print("T1:", end=' ')
                            print(temperature1)

                        # put into ring buffer
                        glob.lockEnvDataRB()
                        glob.putEnvDataRB([float(pressure), float(altitude), temperature1])
                        glob.unlockEnvDataRB()

                    elif (id == 0x06):  # port status data
                        # D0L D0H D1L D1H D2L D2H D3L D3H
                        # 计算方法：
                        # D0(uint16_t) = (uint16_t) (D0H<<8) | D0L;
                        # D1(uint16_t) = (uint16_t) (D1H<<8) | D1L;
                        # D2(uint16_t) = (uint16_t) (D2H<<8) | D2L;
                        # D3(uint16_t) = (uint16_t) (D3H<<8) | D3L;
                        d0 = np.int16((dataList[1] << 8) | dataList[0])
                        d1 = np.int16((dataList[3] << 8) | dataList[2])
                        d2 = np.int16((dataList[5] << 8) | dataList[4])
                        d3 = np.int16((dataList[7] << 8) | dataList[6])
                        if (is_openLog == 1):
                            print("port sta:", end=' ')
                            print(d0, d1, d2, d3)

                    else:  # err, discard data
                        print("mas id is not matched!")

